﻿#include "SemiAutoMeasure.h"
#include "CircleFitSolver_OpenCV.h"
#include "plinesfit_opencv.h"
#include "opencv2/core.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/imgproc.hpp"


#include <QtMath>
namespace Qly {
SemiAutoMeasure::SemiAutoMeasure(MeasureDataSet *dataSet)
{
    pDataSet = dataSet;
}



void drawPoints(cv::Mat mat, std::vector<cv::Point2i> &points, cv::Scalar color)
{
    for(size_t i = 0; i < points.size(); i ++)
    {
        cv::circle(mat, points[i], 2, color, 1);
    }
}

bool SemiAutoMeasure::semiAutoOuterCircle(MeasureCircleCenterSA *element, cv::Mat &edgeMat)
{
    if(element == nullptr) return false;

    const QVector<double> &  data = element->results_pix();
    double x = data.at(0) + edgeMat.cols / 2.0; //xy 是以图像中心作为原点的
    double y = data.at(1) + edgeMat.rows / 2.0;
    double radius = data.at(2);

    QVector<double> dist_data(360, 1e10);
    QVector<int> x_data(360, -1);
    QVector<int> y_data(360, -1);

    for(int row = 0; row < edgeMat.rows; row ++)
    {
        const uchar *pLine = edgeMat.ptr<uchar>(row);
        for(int col = 0; col < edgeMat.cols; col ++)
        {
            if(pLine[col] == 0) continue;
            int dx = col - x;
            int dy = row - y;
            double dist = hypot(dx, dy);

            if(dist < radius) continue;

            double angle = atan2(dy , dx) / M_PI * 180.0 + 180.0;
            int deg = angle;  deg = qBound(0, deg, 359);

            if(dist < dist_data.at(deg))
            {
                dist_data[deg] = dist;
                x_data[deg] = col;
                y_data[deg] = row;
            }
        }
    }

    // 这里还需要把 空的数据去掉
    std::vector<cv::Point2i> points;
//    std::vector<cv::Point2i> points2;
    for(int i = 0; i < 360; i++)
    {
        if(x_data.at(i) != -1)
        {
            points.push_back(cv::Point2i(x_data[i] - edgeMat.cols / 2.0, y_data[i] - edgeMat.rows / 2.0));
//            points2.push_back(cv::Point2i(x_data[i], y_data[i]));
        }
    }
    if(points.size() < 3) return false;
//    cv::Mat cm;
//    cv::cvtColor(grayMat, cm, cv::COLOR_GRAY2BGR);
//    drawPoints(cm, points2, cv::Scalar(255, 255, 0));
//    cv::imshow("edgeMat", cm);
    double centerX, centerY, rad;
    circleLeastFit(points, centerX, centerY, rad);

    element->adjust(centerX, centerY, rad);
    return true;
}


bool SemiAutoMeasure::semiAutoInnerCircle(MeasureCircle2PSA *element, cv::Mat &edgeMat)
{
    if(element == nullptr) return false;

    const QVector<double> &  data = element->results_pix();
    double x = data.at(0) + edgeMat.cols / 2.0; //xy 是以图像中心作为原点的
    double y = data.at(1) + edgeMat.rows / 2.0;
    double radius = data.at(2);

    QVector<double> dist_data(360, 0);
    QVector<int> x_data(360, -1);
    QVector<int> y_data(360, -1);

//    cv::namedWindow("edgeMat", cv::WINDOW_NORMAL);
//    cv::imshow("edgeMat", edgeMat);

    for(int row = 0; row < edgeMat.rows; row ++)
    {
        const uchar *pLine = edgeMat.ptr<uchar>(row);
        for(int col = 0; col < edgeMat.cols; col ++)
        {
            if(pLine[col] == 0) continue;
            int dx = col - x;
            int dy = row - y;
            double dist = hypot(dx, dy);

            if(dist > radius) continue;

            double angle = atan2(dy , dx) / M_PI * 180.0 + 180.0;
            int deg = angle;  deg = qBound(0, deg, 359);

            if(dist > dist_data.at(deg))
            {
                dist_data[deg] = dist;
                x_data[deg] = col;
                y_data[deg] = row;
            }
        }
    }

    // 这里还需要把 空的数据去掉
    std::vector<cv::Point2i> points;
//    std::vector<cv::Point2i> points2;
    for(int i = 0; i < 360; i++)
    {
        if(x_data.at(i) != -1)
        {
            points.push_back(cv::Point2i(x_data[i] - edgeMat.cols / 2.0, y_data[i] - edgeMat.rows / 2.0));
//            points2.push_back(cv::Point2i(x_data[i], y_data[i]));
        }
    }
//    cv::Mat cm;
//    cv::cvtColor(grayMat, cm, cv::COLOR_GRAY2BGR);
//    drawPoints(cm, points2, cv::Scalar(255, 255, 0));

//    const QVector<QPointF> &kp = element->keyPoints_pix();

//    cv::circle(cm, cv::Point2i(kp[0].x() + grayMat.cols / 2.0, kp[0].y() + grayMat.rows / 2.0), 3, cv::Scalar(255, 0, 0));
//    cv::circle(cm, cv::Point2i(kp[1].x() + grayMat.cols / 2.0, kp[1].y() + grayMat.rows / 2.0), 3, cv::Scalar(255, 0, 0));

//    cv::circle(cm, cv::Point2i(x, y), radius, cv::Scalar(0, 0, 255), 2);
//    cv::imshow("2", cm);

    if(points.size() < 3) return false;
    double centerX, centerY, rad;
    circleLeastFit(points, centerX, centerY, rad);

    element->adjust(QPointF(centerX - rad, centerY), QPointF(centerX + rad, centerY));
    return true;

}

/**
 * @brief footOfPerpendicular 点 p 到直线的垂足
 * @param p 点p
 * @param a 直线方程 ax + by + c = 0
 * @param b
 * @param c
 * @return 垂足坐标
 */
QPointF footOfPerpendicular(QPointF &p, double a, double b, double c)
{
 //   double x0 = p.x(), y0 = p.y();

    //(x - x0) * b + (y - y0) * a = 0;
    double x = (b * b * p.x()- a * b * p.y() - a * c)/(a * a + b * b);
    double y = (-a * b * p.x() + a * a  * p.y() - b * c) / (a * a + b * b);
    return QPointF(x, y);
}

bool SemiAutoMeasure::semiAutoLineWidth(MeasureLineWidthSA *element, cv::Mat &edgeMat)
{
    if(element == nullptr) return false;

    QPointF p1 = element->keyPoints_pix().at(0) ;
    QPointF p2 = element->keyPoints_pix().at(1) ;

    double length_pix = hypot( p1.x() - p2.x(), p1.y() - p2.y() );
    QPointF center = (p1 + p2) / 2.0 + QPointF(edgeMat.cols / 2.0, edgeMat.rows / 2.0); //转换为图像坐标系

    double width_pix = element->results_pix().at(0);
    double cos_theta = element->results_pix().at(1);
    double sin_theta = element->results_pix().at(2);
    double theta = atan2(sin_theta, cos_theta);

    std::vector<cv::Point2d> edgePoints1, edgePoints2;
    for(int row = 0; row < edgeMat.rows; row ++)
    {
        const uchar *pLine = edgeMat.ptr<uchar>(row);
        for(int col = 0; col < edgeMat.cols; col ++)
        {
            if(pLine[col] == 0) continue; //不是边缘，自然不需要处理

            // 判断这个点是否在范围内
            double x = col - center.x();
            double y = row - center.y();

            double xx = x * cos_theta + y * sin_theta;
            double yy =-x * sin_theta + y * cos_theta;

            if(fabs(xx) > length_pix / 2.0 || fabs(yy) > width_pix / 2.0 ) continue;
            // 剩下的点在这个范围内
            if(yy > 0)
            {
                edgePoints1.push_back(cv::Point2i(col, row));
            }
            else
            {
                edgePoints2.push_back(cv::Point2i(col, row));
            }


        }
    }

//        cv::Mat cm;
//        cv::cvtColor(edgeMat, cm, cv::COLOR_GRAY2BGR);
//        drawPoints(cm, edgePoints1, cv::Scalar(255, 255, 0));
//        drawPoints(cm, edgePoints2, cv::Scalar(0, 255, 0));
//        cv::imshow("edgeMat", cm);

    if(edgePoints1.size() < 2 || edgePoints2.size() < 2) return false;
    double a, b, c1, c2, c;
    plinesFit(edgePoints1, edgePoints2, a,  b, c1, c2);


    QPointF fop1 = footOfPerpendicular(center, a, b, c1);
    QPointF fop2 = footOfPerpendicular(center, a, b, c2);
    center = (fop1 + fop2) / 2.0;//根据算出的两条平行线调整中心点的位置。
    center = center - QPointF(edgeMat.cols / 2.0, edgeMat.rows / 2.0);

    c = (c1 + c2) / 2.0;
    width_pix = fabs(c2 - c1);

    p1.setX(center.x() + length_pix * b / 2.0);
    p1.setY(center.y() - length_pix * a / 2.0);

    p2.setX(center.x() - length_pix * b / 2.0);
    p2.setY(center.y() + length_pix * a / 2.0);

    element->adjust(p1, p2, width_pix);
    return true;
}

void SemiAutoMeasure::semiAutoMeasure(MeasureElement *element, cv::Mat &grayMat)
{
    cv::Mat edgeMat;
    cv::Canny(grayMat, edgeMat, 40, 120);
    if(element->type() == Qly::MeasureElement::CircleCenter_SA)
    {
        qDebug() << "here";
        Qly::MeasureCircleCenterSA * pCC = dynamic_cast<Qly::MeasureCircleCenterSA *>(element);
        if( !semiAutoOuterCircle(pCC, edgeMat) )
        {
            if(pDataSet) pDataSet->deleteElement(element);
        }
        return;
    }
    else if(element->type() == Qly::MeasureElement::Circle2P_SA)
    {
        Qly::MeasureCircle2PSA * pC2 = dynamic_cast<Qly::MeasureCircle2PSA *>(element);
        if(!semiAutoInnerCircle(pC2, edgeMat))
        {
            if(pDataSet) pDataSet->deleteElement(element);
        }
        return;
    }
    else if(element->type() == Qly::MeasureElement::LineWidth_SA)
    {
        Qly::MeasureLineWidthSA *pLW = dynamic_cast<Qly::MeasureLineWidthSA *>(element);
        if(!semiAutoLineWidth(pLW, edgeMat))
        {
            if(pDataSet) pDataSet->deleteElement(element);
        }
        return;
    }
}

}

